//------------------------------------------------------------------------------
// Includes 
//------------------------------------------------------------------------------
#include <OpenCV/cv.h>
#include <Tracker/SimpleManeuveringFilter.h>

namespace Tracker
{
	SimpleManeuveringFilter::SimpleManeuveringFilter(int measurementDimension) :
	    kalman(NULL), measurementDimension(measurementDimension), 
	    stateDimension(measurementDimension*2) {
	
	    kalman = cvCreateKalman(stateDimension, measurementDimension, 0);
	
	    setManeuveringTime();
	    setProcessStdDev();
	    setMeasurementStdDev();
	}
	
	SimpleManeuveringFilter::~SimpleManeuveringFilter() {
	    cvReleaseKalman(&kalman);
	}
	
	void SimpleManeuveringFilter::init(const double state0[]) {
	    double dt = 1; // time interval between executions... 1 for "one run"...
	    double dt2 = dt*dt;
	    double dt3 = dt*dt*dt;
	
	    // set up transition matrix A
	    // time correlated velocity... will slow down with time
	    // and have mostly stopped at time tau = "maneuvering time"
	    CvMat *A = kalman->transition_matrix;
	    cvSetZero(A);
	    for (int i = 0; i < measurementDimension; i++) {
	        int o = i*2; // origin of submatrix
	        cvmSet(A, o  , o, 1);  cvmSet(A, o  , o+1, (1-exp(-dt/tau))*tau);
	        cvmSet(A, o+1, o, 0);  cvmSet(A, o+1, o+1,    exp(-dt/tau));
	    }
	
	    // set up measurement matrix H 
	    CvMat *H = kalman->measurement_matrix;
	    cvSetZero(H);
	    for (int i = 0; i < measurementDimension; i++) {
	        cvmSet(H, i, i*2, 1); cvmSet(H, i, i*2+1, 0);
	    }
	
	    // set up process noise matrix Q 
	    // using Taylor series approximations only...
	    double c = 2*sProcess*sProcess/tau;
	    CvMat *Q = kalman->process_noise_cov;
	    cvSetZero(Q);
	    for (int i = 0; i < measurementDimension; i++) {
	        int o = i*2; // origin of submatrix
	        cvmSet(Q, o  , o, c*dt3/3);  cvmSet(Q, o  , o+1, c*dt2/2);
	        cvmSet(Q, o+1, o, c*dt2/2);  cvmSet(Q, o+1, o+1, c*dt);
	    }
	
	    // set up measurement noise R
	    CvMat *R = kalman->measurement_noise_cov;
	    cvSetZero(R);
	    for (int i = 0; i < measurementDimension; i++) {
	        cvmSet(R, i, i, sMeasurement*sMeasurement);
	    }
	
	    // set up initial state
	    setState(state0);
	
	    // here, the initial error is estimated to the process noise...
	    cvCopy(kalman->process_noise_cov, kalman->error_cov_post);
	}
	
	void SimpleManeuveringFilter::run(const double measurement[]) {
	    cvKalmanPredict(kalman);
	
	    if (measurement == NULL) {
	        // no measurement -> operate without correction 
	        cvCopy(kalman->state_pre, kalman->state_post);
	        cvCopy(kalman->error_cov_pre, kalman->error_cov_post);
	    } else {
	        // otherwise, run correction phase...
	        float z_data[measurementDimension];
	        for (int i = 0; i < measurementDimension; i++)
	            z_data[i] = measurement[i];
	        CvMat z = cvMat(measurementDimension, 1, CV_32FC1, (void*)z_data);
	        cvKalmanCorrect(kalman, &z);
	    }
	}
	
	void SimpleManeuveringFilter::getState(double state[]) const {
	    CvMat *X = kalman->state_post;
	    for (int i = 0; i < stateDimension; i++) {
	        state[i] = cvmGet(X, i, 0);
	        if (isnan(state[i])) {
	             // wtf happened?? 
	             // well, let's at least put something sensible here. i.e.: 0
	            cvmSet(X, i, 0, 0);
	            state[i] = 0;
	        }
	    }
	}
	
	void SimpleManeuveringFilter::setState(const double state[]) {
	    CvMat *X = kalman->state_post;
	    for (int i = 0; i < stateDimension; i++) {
	        cvmSet(X, i, 0, state[i]);
	    }
	}
}
